package ai;

import java.sql.Date;
import java.util.ArrayList;
import mathematics.geometry.Geometry;
import mathematics.geometry.SegmentGeometry;
import matrix.GraphDoor;
import matrix.GraphNode;
import model.RoomModel;
import model.path.Path;
import model.path.Point4D;
import utils.Point3D;
import utils.Vector3D;

public class PathFollowingBehaviour 
{
	public static Point3D position = new Point3D(0,0);
	private static Vector3D velocity = new Vector3D();
	private static Vector3D steerForce = new Vector3D();
	private static Vector3D acceleration = new Vector3D();
	private static Vector3D wanderTarget = new Vector3D();

	private static float mass = 2;
	private static float maxSpeed = 5;
	private static float maxForce = 10;
	
	private static float wanderRadius = 20; //the radius of the circle
	private static float wanderDistance = 50;
	private static float wanderJitter = 8;

	private static Point3D circleCenterM = new Point3D();

	private static Vector3D pointOnCircle = new Vector3D();
	
	public static float interestPointRadius = 20;


	public PathFollowingBehaviour()
	{
	}	

	
	private static Point3D nextPathFollowingPosition(Geometry geometricShape, Point3D entryPoint, Point3D endPoint) 
	{
		Vector3D newSteerForce = PathFollowing(endPoint);
		newSteerForce = Vector3D.Truncate(newSteerForce, PathFollowingBehaviour.maxForce);
		PathFollowingBehaviour.steerForce = newSteerForce;

		PathFollowingBehaviour.acceleration = Vector3D.mult(newSteerForce, 1.0f / PathFollowingBehaviour.mass);
		PathFollowingBehaviour.velocity = Vector3D.Truncate(Vector3D.sum(PathFollowingBehaviour.velocity, PathFollowingBehaviour.acceleration), PathFollowingBehaviour.maxSpeed);
		
		Vector3D heading = PathFollowingBehaviour.velocity.normalized();
		 
		Point3D newPosition = Point3D.sum(entryPoint, PathFollowingBehaviour.acceleration);
		Point3D collisionPoint = Point3D.sum(entryPoint,Vector3D.mult(heading, 20 + Math.random() * 5));
		
	
		SegmentGeometry seg = new SegmentGeometry(entryPoint,collisionPoint);

		if(seg.isIntersectedBy(geometricShape))
		{
			PathFollowingBehaviour.position = entryPoint;
			return null;
		}
		PathFollowingBehaviour.position = newPosition;
		
		return newPosition;
	}

	private static Vector3D PathFollowing(Point3D endPoint) 
	{
		Vector3D heading = PathFollowingBehaviour.steerForce.normalized();
		Vector3D globalDirection = new Vector3D(PathFollowingBehaviour.position, endPoint);
		globalDirection = globalDirection.normalized();
		globalDirection = Vector3D.mult(globalDirection, 2);
		
		double randX = (Math.random() * 2.0 - 1) * PathFollowingBehaviour.wanderJitter;
		double randY = (Math.random() * 2.0 - 1) * PathFollowingBehaviour.wanderJitter;

		PathFollowingBehaviour.wanderTarget = Vector3D.sum(PathFollowingBehaviour.wanderTarget, new Vector3D((float)randX, (float)randY));
		PathFollowingBehaviour.wanderTarget = PathFollowingBehaviour.wanderTarget.normalized();
		PathFollowingBehaviour.wanderTarget =  Vector3D.mult(PathFollowingBehaviour.wanderTarget, PathFollowingBehaviour.wanderRadius / 2.0f);

		PathFollowingBehaviour.circleCenterM = new Point3D((heading.x() * PathFollowingBehaviour.wanderDistance) + PathFollowingBehaviour.position.x(), (heading.y() * PathFollowingBehaviour.wanderDistance) + PathFollowingBehaviour.position.y());
		PathFollowingBehaviour.pointOnCircle = new Vector3D(PathFollowingBehaviour.circleCenterM.x() + PathFollowingBehaviour.wanderTarget.x(), PathFollowingBehaviour.circleCenterM.y() + PathFollowingBehaviour.wanderTarget.y());

		Vector3D result = Vector3D.sum(PathFollowingBehaviour.pointOnCircle, new Vector3D(- PathFollowingBehaviour.position.x(), - PathFollowingBehaviour.position.y()));

		double length = result.length();
		
		result = result.normalized();
		result = Vector3D.sum(result, globalDirection);
		result = Vector3D.mult(result, length);
		
		return result;	
	}
/*
	public static ArrayList<Point4D> generateBehaviour(RoomModel room, ArrayList<PathStopPoint> positions) 
	{
		ArrayList<Point4D> trajectory = new ArrayList<Point4D>();
	
		long totalLength = 0;
		long totalTime = 0;
		long startTime = positions.get(0).getTimeIntervalle().endTime;
		long endTime = positions.get(positions.size()-1).getTimeIntervalle().beginTime;
		
		Vector3D initialisingVector = (new Vector3D(positions.get(0).getNode().getPosition(), positions.get(1).getNode().getPosition())).normalized();
		Point3D lastComputedPosition = Point3D.sum(positions.get(0).getNode().getPosition(), Vector3D.mult(initialisingVector, 1));
	
		long currentTime = startTime;
		
		for(int i = 0; i < positions.size() - 1; i++)
		{
			Point3D currentEndPoint = positions.get(i).getNode().getPosition();
			
			Vector3D vectorPositionToEndPoint = new Vector3D(lastComputedPosition, currentEndPoint);
			double length = vectorPositionToEndPoint.length();
					
			float range = Settings.checkPointRadius;

			Point3D nextComputedPosition;
			
			while(length >= range)
			{		
				nextComputedPosition = PathFollowingBehaviour.nextPathFollowingPosition(room.getGeometry(), lastComputedPosition, currentEndPoint);
				int j = 0;
				while(nextComputedPosition == null && j < 10)
				{
					j++;
					nextComputedPosition = PathFollowingBehaviour.nextPathFollowingPosition(room.getGeometry(), lastComputedPosition, currentEndPoint);
				}	
				if(nextComputedPosition != null)
				{
					double lengthBetween2Positions = Point3D.diff(nextComputedPosition, lastComputedPosition).length();
					totalLength += lengthBetween2Positions;
					
					long timeStep = (long) ((lengthBetween2Positions / Settings.walkingSpeed) * 1000);
					totalTime += timeStep;
					
					currentTime += timeStep;

					Point4D point = new Point4D(nextComputedPosition, new Date(currentTime));
					point.setRoom(room);
					point.setTime(new Date(currentTime));

					trajectory.add(point);
					
					vectorPositionToEndPoint = new Vector3D(nextComputedPosition, currentEndPoint);
					length = vectorPositionToEndPoint.length();
					lastComputedPosition = nextComputedPosition;
				}
			}
		}

		return trajectory;
	}*/


	public static ArrayList<Point4D> generateBehaviour(Path path, long beginTime) {
		// TODO Auto-generated method stub
		ArrayList<Point4D> trajectory = new ArrayList<Point4D>();
		
		long startTime = beginTime;
		
		Vector3D initialisingVector = (new Vector3D(path.get(0).getNode().getPosition(), path.get(1).getNode().getPosition())).normalized();
		Point3D lastComputedPosition = Point3D.sum(path.get(0).getNode().getPosition(), Vector3D.mult(initialisingVector, 1));
	
		long currentTime = startTime;

		GraphDoor lastDoor = null;
		
		for(int i = 0; i < path.size() - 1; i++)
		{
			Point3D currentEndPoint = path.get(i).getNode().getPosition();
			
			Vector3D vectorPositionToEndPoint = new Vector3D(lastComputedPosition, currentEndPoint);
			double length = vectorPositionToEndPoint.length();
					
			float range = Settings.checkPointRadius;

			Point3D nextComputedPosition;
			
			RoomModel room =  path.get(i).getNode().getRoom();
			if(path.get(i).getNode().getClass() == GraphDoor.class)
			{	
				GraphDoor graphDoor = (GraphDoor)path.get(i).getNode();

				lastDoor = graphDoor;
				
				Point3D doorPosition = graphDoor.getPosition();
				
				double lengthBetween2Positions = Point3D.diff(doorPosition, lastComputedPosition).length();
				
				long timeStep = (long) ((lengthBetween2Positions / Settings.walkingSpeed) * 1000);
				
				currentTime += timeStep;

				
				Point4D doorPoint = new Point4D(doorPosition, new Date(currentTime));
				doorPoint.setRoom(room);

				lastComputedPosition = doorPosition;
				trajectory.add(doorPoint);	
			}
			else
			{
				if(lastDoor != null)
				{
					GraphNode nextNode = (GraphNode) (room.getMatrix().getNeighbours(lastDoor).get(0));	
					
					Point3D nextNodePosition = nextNode.getPosition();
					double lengthBetween2Positions = Point3D.diff(nextNodePosition, lastComputedPosition).length();
					
					long timeStep = (long) ((lengthBetween2Positions / Settings.walkingSpeed) * 1000);
					
					currentTime += timeStep;
					Point4D doorPoint = new Point4D(nextNodePosition, new Date(currentTime));
					doorPoint.setRoom(room);

					lastComputedPosition = nextNodePosition;
					trajectory.add(doorPoint);		
					lastComputedPosition = nextNode.getPosition();
				}
				while(length >= range)
				{	
					nextComputedPosition = PathFollowingBehaviour.nextPathFollowingPosition(room.getGeometry(), lastComputedPosition, currentEndPoint);
					int j = 0;
					while(nextComputedPosition == null && j < 10)
					{
						j++;
						nextComputedPosition = PathFollowingBehaviour.nextPathFollowingPosition(room.getGeometry(), lastComputedPosition, currentEndPoint);
					}	
					if(nextComputedPosition != null)
					{
						double lengthBetween2Positions = Point3D.diff(nextComputedPosition, lastComputedPosition).length();
					
						long timeStep = (long) ((lengthBetween2Positions / Settings.walkingSpeed) * 10000);
						
						currentTime += timeStep;
	
						Point4D point = new Point4D(nextComputedPosition, new Date(currentTime));
						point.setRoom(room);
						point.setTime(new Date(currentTime));
	
						trajectory.add(point);
						
						vectorPositionToEndPoint = new Vector3D(nextComputedPosition, currentEndPoint);
						length = vectorPositionToEndPoint.length();
						lastComputedPosition = nextComputedPosition;
					}
				}
				lastDoor = null;
			}
			currentTime += path.get(i).getStayTime();
		}
		
		return trajectory;
	}
}
